/*** 
 * @file: chassis_task.cpp
 * @brief: 底盘解算任务
 * @attention: 三角舵轮底盘逆运动学解算及电机控制任务
 *             底盘构型为：
 *                              前 Y
 *                              O-----O
 *                    左 -X      \   /      右 X
 *                                \ /                  偏航角Yaw 为Z 逆时针为正
 *                                 O        
 *                               后 -Y
 * 
 *             底盘正运动学解算函数未使用
 * @LastEditTime: 2025-11-01 22:15:28
 */

#include "chassis_task.h"
#include "cmath"
/*====================  配置常量  ====================*/
namespace cfg {
    constexpr float Max_speed = 3000.0f;
    constexpr float MAX_CMD_VALUE = 10.0f;
    constexpr float CMD_TO_RPM = Max_speed / MAX_CMD_VALUE;
    
    constexpr float PI = 3.14159265358979323846f;
    constexpr float DEG2RAD = PI / 180.0f;
    constexpr float RAD2DEG = 180.0f / PI;
    
    // 120°布局的固定三角函数值 (编译期计算)
    constexpr float SIN_30 = 0.5f;
    constexpr float COS_30 = 0.86602540378f;  // √3/2
}


//电机索引常量 - 用于 can1_pointer 数组索引
namespace motor_index {
    constexpr int STEER_LF = 0;  // 左前舵向
    constexpr int STEER_RF = 1;  // 右前舵向
    constexpr int STEER_H  = 2;  // 后舵向
    constexpr int WHEEL_LF = 3;  // 左前轮
    constexpr int WHEEL_RF = 4;  // 右前轮  
    constexpr int WHEEL_H  = 5;  // 后轮
}
 /*	外部变量&头文件链接区 START	*/  

extern  Can_motor   m_0x201_can1;       //左前舵向6220
extern  Can_motor   m_0x202_can1;       //右前舵向6220
extern  Can_motor   m_0x203_can1;       //后舵向6220
extern  Can_motor   m_0x204_can1;       //左前轮向3508
extern  Can_motor   m_0x205_can1;       //右前轮向3508
extern  Can_motor   m_0x206_can1;       //后轮向3508

extern remote_control_t remote_control;    //遥控器数据

 /*	外部变量&头文件链接区 END	*/

static Can_motor* can1_pointer[6];  //解耦指针数组
static remote_control_t* remote_pointer;    //遥控器数据

 typedef struct
 {  
    //车速
     float Vx;
     float Vy; 
     float Vz; //Yaw轴 逆时针旋转为正
    //轮速&舵向角度
     float Steer_LF_angle;//左前舵向角度
     float Steer_RF_angle;//右前舵向角度
     float Steer_H_angle;//后舵向角度

     float Wheel_LF_speed;//左前轮速度
     float Wheel_RF_speed;//右前轮速度
     float Wheel_H_speed;//后轮速度
     
 }Chassis_speed_t;

 Chassis_speed_t Chassis_act_speed;
 Chassis_speed_t Chassis_target_speed;

//RTOS任务参数
osThreadId_t ChassisTaskHandle;
osThreadAttr_t ChassisTask_attributes;

extern "C" void chassis_task_init();
extern "C" void ChassisTask(void *argument);
void Chassis_inverse_kinematics(int Vx,int Vy,int Vz, Chassis_speed_t *chassis_target_speed);
void Chssis_set_motor(Chassis_speed_t *chassis_target_speed , 
                      Chassis_speed_t *chassis_act_speed
                  );

 /*** 
  * @description: 创建舵轮底盘任务
  * @return {*}
  */ 
 extern "C" void chassis_task_init()
 {
    ChassisTask_attributes.name = "ChassisTask";
    ChassisTask_attributes.stack_size = 1024 * 4;
    ChassisTask_attributes.priority = (osPriority_t) osPriorityNormal;
    ChassisTaskHandle = osThreadNew((osThreadFunc_t)ChassisTask, NULL, &ChassisTask_attributes);
 }

 
 /*** 
  * @description: 舵轮底盘任务
  * @param {void} *argument
  * @return {*}
  */
 extern "C" void ChassisTask(void *argument)
    {
        //解耦  如电机id更改只需更改此处即可
        can1_pointer[0] = &m_0x201_can1;//左前舵向6220
        can1_pointer[1] = &m_0x202_can1;//右前舵向6220
        can1_pointer[2] = &m_0x203_can1;//后舵向6220
        can1_pointer[3] = &m_0x204_can1;//左前轮向3508
        can1_pointer[4] = &m_0x205_can1;//右前轮向3508
        can1_pointer[5] = &m_0x206_can1;//后轮向3508
        remote_pointer = &remote_control;
        for(;;)
{
    Chassis_inverse_kinematics(remote_pointer->x,
    remote_pointer->y,remote_pointer->z, &Chassis_target_speed);
    Chssis_set_motor(&Chassis_target_speed, &Chassis_act_speed); 
    osDelay(2);   //500hz
}
    }

    /*** 
     * @description: 输入目标并进行底盘逆运动学解算，由底盘速度解算出轮速和舵向角度
     * @param {chassis_target_speed_t} *chassis_target_speed  
     * @return {*}
     */
		void Chassis_inverse_kinematics(int Vx,int Vy,int Vz, Chassis_speed_t *chassis_target_speed)
		{
        // 修正：使用正确的指针访问方式
        if (Vx == 0 && Vy == 0 && Vz == 0)
        {
						chassis_target_speed->Vx = chassis_target_speed->Vy 
						= chassis_target_speed->Vz = 0;
            chassis_target_speed->Wheel_LF_speed = 0;
            chassis_target_speed->Wheel_RF_speed = 0;
            chassis_target_speed->Wheel_H_speed  = 0;
            return;
        }
        
				//由于遥控器指令范围是 -10 到 10 ，因此要乘一个倍率
				float k = cfg::Max_speed /10;
        chassis_target_speed->Vx = Vx * k;
        chassis_target_speed->Vy = Vy * k * -1;//因为XBOX往上是负Y往下Y，需要取反 ！！！！ 根据设置
        chassis_target_speed->Vz = Vz * k;
				
				
				const float Vz_sin30 = chassis_target_speed->Vz * cfg::SIN_30;
				const float Vz_cos30 = chassis_target_speed->Vz * cfg::COS_30;
        
				// std::hypot 替代手动平方根
				chassis_target_speed->Wheel_LF_speed = std::hypot(
				chassis_target_speed->Vx - Vz_sin30, 
        chassis_target_speed->Vy - Vz_cos30
				);
    
				chassis_target_speed->Wheel_RF_speed = std::hypot(
        chassis_target_speed->Vx + Vz_sin30, 
        chassis_target_speed->Vy - Vz_cos30
    );
    
				chassis_target_speed->Wheel_H_speed = std::hypot(
        chassis_target_speed->Vx, 
        chassis_target_speed->Vy + chassis_target_speed->Vz
    );

    //  std::atan2 替代 atan2f
				chassis_target_speed->Steer_LF_angle = std::atan2(
				chassis_target_speed->Vy - Vz_cos30,
        chassis_target_speed->Vx - Vz_sin30
    ) * cfg::RAD2DEG;
    
				chassis_target_speed->Steer_RF_angle = std::atan2(
        chassis_target_speed->Vy - Vz_cos30,
        chassis_target_speed->Vx + Vz_sin30
    ) * cfg::RAD2DEG;
    
				chassis_target_speed->Steer_H_angle = std::atan2(
        chassis_target_speed->Vy,
        chassis_target_speed->Vx
    ) * cfg::RAD2DEG;




		}

    /*** 
     * @description: 更新更新电机状态，并发送电机控制指令，
     * @param {Chassis_speed_t} *chassis_target_speed
     * @param {Chassis_speed_t} *chassis_act_speed
     * @param {Can_motor} *m_0x20(n)_can1 对应各个电机控制指针
     * @return {*}
     */    
    void Chssis_set_motor(Chassis_speed_t *chassis_target_speed, 
                      Chassis_speed_t *chassis_act_speed)
{
    // 读取实际状态
    chassis_act_speed->Steer_LF_angle = can1_pointer[0]->angle_now;
    chassis_act_speed->Steer_RF_angle = can1_pointer[1]->angle_now;
    chassis_act_speed->Steer_H_angle  = can1_pointer[2]->angle_now;
    chassis_act_speed->Wheel_LF_speed = can1_pointer[3]->speed_rpm_now;
    chassis_act_speed->Wheel_RF_speed = can1_pointer[4]->speed_rpm_now;
    chassis_act_speed->Wheel_H_speed  = can1_pointer[5]->speed_rpm_now;
    
    // 发送舵向角度命令
    can1_pointer[0]->target_angle = chassis_target_speed->Steer_LF_angle;
    can1_pointer[1]->target_angle = chassis_target_speed->Steer_RF_angle;
    can1_pointer[2]->target_angle = chassis_target_speed->Steer_H_angle;
    
    // 发送轮速命令
    can1_pointer[3]->target_speed = chassis_target_speed->Wheel_LF_speed;
    can1_pointer[4]->target_speed = chassis_target_speed->Wheel_RF_speed;
    can1_pointer[5]->target_speed = chassis_target_speed->Wheel_H_speed;
}

    /*** 
		* @description: 底盘正运动学解算，由轮速和舵向角度计算出底盘xy速度            !!!!!!!!未使用!!!!!!!!!
     * @param {Chassis_speed_t} *chassis_speed
     * @return {*}
     */    
    void Chassis_forward_kinematics(Chassis_speed_t *chassis_speed);